








#include <ros/ros.h>


#include "px4_msgs/Px4ControlCommand.h"


using namespace std;

px4_msgs::Px4ControlCommand Planner_Command;

int command_id = 1;  //保证第一个任务会被接收到

ros::Publisher px4_planner_pub;

void send_planner_mission();

int main(int argc, char** argv)
{
    ros::init(argc, argv, "planner_mission");
    ros::NodeHandle nh("~");


    px4_planner_pub = nh.advertise<px4_msgs::Px4ControlCommand>("/px4/control_command", 10);    


    ros::Rate rate(50.0);

    while(ros::ok())
    {
        ros::spinOnce();       
        send_planner_mission();
        rate.sleep();
    }

    return 0;

}

void send_planner_mission()
{
    Planner_Command.header.stamp = ros::Time::now();
    Planner_Command.header.frame_id = "'world";

    Planner_Command.Command_ID = command_id;
    Planner_Command.Mode = px4_msgs::Px4ControlCommand::Takeoff;
    ///
    cout << "Command_id: " << Planner_Command.Command_ID  << endl;
    px4_planner_pub.publish(Planner_Command);
    command_id ++;
}





